/*
 * Entech2013DriveSubsytem.hpp
 *
 *  Created on: Feb 3, 2013
 *      Author: N500432
 */
#ifndef _DRIVE_SUBSYSTEM_H
#define _DRIVE_SUBSYSTEM_H
/*
 * Description:
 */

#include <WPILib.h>
#include "EntechSubsystem.h"

class Entech2013DriveSubsystem : public EntechSubsystem
{
public:
    Entech2013DriveSubsystem();
    virtual ~Entech2013DriveSubsystem();

    enum DriveType { kTankDrive, kArcadeDrive };
    enum ShifterState { kLowGear, kHighGear };
    
    // Subsystem functions
    virtual void DoRobotInit(void);
    virtual void DoDisabledInit(void);
    virtual void DoAutonomousInit(void);
    virtual void DoTeleopInit(void);
    virtual void DoDisabled(void);
    virtual void DoAutonomous(void);
    virtual void DoTeleop(void);

    // Specific Commands - Autonomous Needs
    void Turn(double angle);
    void StraightDistance(double distance, double speed);
    void StraightTime(double time, double speed);
    void Stop(void);
    void Off(void);
    bool CommandDone(void);
    
    // Other Commands
    void ForceOn(void);
    void ToggleDriveType(void);
    void DoShifter(void);
    void ShiftHigh(void);
    void ShiftLow(void);
    void JoystickDrive(Joystick *js_left, Joystick *js_right);

private:
    double ApplyDeadzone(double input, double deadzone);
    void SetDriveMotors(double left, double right);
    void AutomaticDrive(void);

    enum DriveSystemState { kJoystick, kTurning, kStraight, kStopped, kTimed, kOff };

    // Input Sources
    Joystick *m_leftStick;
    Joystick *m_rightStick;
    
    // Systems we have control over
    Talon *m_leftMotor;
    Talon *m_rightMotor;
    Encoder *m_leftEnc;
    Encoder *m_rightEnc;
    Solenoid *m_highShifterSolenoid;
    Timer *m_timer;

    // Internal state variables
    DriveType m_driveType;
    ShifterState m_shifterState;
    DriveSystemState m_state;
    
    // Motor Sync factors
    double m_motorSpeedDelta;
    double m_leftEncLastPos;
    double m_rightEncLastPos;

    // Autodrive parameters
    double m_leftEncTarget;
	double m_rightEncTarget;
	double m_reqSpeed;
	double m_timeout;
	
	// Numbers are based on 250 counts/rev encoders, 6" dia wheels, 31" wheelbase 
	static const double c_encoderDistancePerDeg = ((31.0 * 3.14159) / 360.0);
	static const double c_distancePerEncoderPulse = ((6.0 * 3.14159) / 250.0);
	
    static const double c_left_inverter = 1.0;
    static const double c_right_inverter = 1.0;
};

#endif
